package com.googlecode.mbhsrobopro.controller;

import java.awt.BorderLayout;
import java.awt.Dimension;
import java.awt.FlowLayout;
import java.awt.GridLayout;
import java.awt.Point;
import java.awt.Toolkit;
import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Calendar;
import java.util.HashMap;
import java.util.Timer;
import java.util.TimerTask;

import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JOptionPane;
import javax.swing.JPanel;

import com.googlecode.mbhsrobopro.controller.components.ProgramController;

import edu.mbhs.fischertechnik.FTController;
import edu.mbhs.fischertechnik.Input;
import edu.mbhs.fischertechnik.Motor;

public class SimpleCombinedController extends JFrame implements KeyListener {

	private static final String dataDir = "data/";
	public static final int FRONT_LEFT = 0, FRONT_RIGHT = 1, CENTER = 2;
	public static final String[] labels = {"Time", "Thermistor", "Left Photocell", 
		"Right Photocell", "Center Photocell"};
	public static final int NUM_LABELS = labels.length;
	public static HashMap<String, ArrayList<Double>> data = new HashMap<String, ArrayList<Double>>();

	public static final int DELAY = 1000;
	public static final int INTERVAL = 1000;
	public static final double THRESHOLD_ANGLE = Math.toRadians(15);
	
	private static double timeCounter = 0;

	private JLabel thermistor;
	private JLabel photocell1, photocell2, photocell3;
	private JLabel motorSpeed;
	
	private boolean right = false, left = false, forward = false, backward = false;

	private int motorValue = 512;

	public SimpleCombinedController() {
		super("RoboPro Wireless Interaction Controller");

		JPanel readingsBorder = new JPanel(new FlowLayout(10, 10, FlowLayout.LEFT));
		JPanel readings = new JPanel(new GridLayout(6, 1));
		motorSpeed = new JLabel("Motor Speed: " + motorValue);
		
		thermistor = new JLabel("Thermistor Reading: ");
		
		photocell1 = new JLabel("Front Left Photocell Reading: ");
		photocell2 = new JLabel("Front Right Photocell Reading: ");
		photocell3 = new JLabel("Center Photocell Reading: ");

		readings.add(motorSpeed);
		readings.add(thermistor);
		readings.add(photocell1);
		readings.add(photocell2);
		readings.add(photocell3);

		readingsBorder.add(readings);

		addKeyListener(this);

		JPanel pane = new JPanel();
		pane.setLayout(new BorderLayout());
		pane.add(readingsBorder, BorderLayout.CENTER);

		add(pane);
		
		setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		addWindowListener(new WindowAdapter() {
			public void windowClosing(WindowEvent e) {
				writeToFile();
			}
		});
		
		setSize(250, 125);

		// Center on Screen
		Dimension size = Toolkit.getDefaultToolkit().getScreenSize();
		setLocation(new Point(size.width / 2 - this.getWidth() / 2, size.height / 2 - this.getHeight() / 2));

		setResizable(false);
		setVisible(true);
		requestFocus();
	}
	
	public static void main(String[] args) {
		final FTController c = new FTController();
		c.openController("COM5");
		
		SimpleCombinedController controller = new SimpleCombinedController();

		// Periodic Data Collect
		for (int i = 0; i < labels.length; i++)
			data.put(labels[i], new ArrayList<Double>());
		
		Timer timer = new Timer();

		timer.scheduleAtFixedRate(new TimerTask() {
			public void run() {
				addDataPoint(labels[0], timeCounter);
				addDataPoint(labels[1], (double) c.getAnalog(Input.I5));
				addDataPoint(labels[2], (double) c.getAnalog(Input.I2));
				addDataPoint(labels[3], (double) c.getAnalog(Input.I3));
				addDataPoint(labels[4], (double) c.getAnalog(Input.I1));
				
				timeCounter++;
			}
		}, DELAY, INTERVAL);

		// Rover Control
		while (true) {
			controller.setThermistorReading(c.getAnalog(Input.I5));
			controller.setPhotocellReading(c.getAnalog(Input.I2), FRONT_LEFT);
			controller.setPhotocellReading(c.getAnalog(Input.I3), FRONT_RIGHT);
			controller.setPhotocellReading(c.getAnalog(Input.I1), CENTER);
			
			c.setMotor(480, Motor.M4);

			if (controller.isForward()) {
				c.setMotor(controller.getMotorSpeed(), Motor.M1);
				c.setMotor(controller.getMotorSpeed(), Motor.M2);
			} else if (controller.isBackward()) {
				c.setMotor(-controller.getMotorSpeed(), Motor.M1);
				c.setMotor(-controller.getMotorSpeed(), Motor.M2);
				c.setMotor(512, Motor.M3);
			} else if (controller.isRight()) {
				c.setMotor(-controller.getMotorSpeed(), Motor.M1);
				c.setMotor(controller.getMotorSpeed(), Motor.M2);

			} else if (controller.isLeft()) {
				c.setMotor(controller.getMotorSpeed(), Motor.M1);
				c.setMotor(-controller.getMotorSpeed(), Motor.M2);
			} else {
				c.setMotor(0, Motor.M1);
				c.setMotor(0, Motor.M2);
				c.setMotor(0, Motor.M3);
			}
		}
	}
	
	public static void sleep() {
		
	}
	
	public static void sleep(double seconds) {
		try {
			Thread.sleep((long) (seconds * 1000));
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	public static String getHeaderLine() {
		String header = "";

		for (String s : labels)
			header += s + "\t";

		return header.substring(0, header.lastIndexOf("\t")) + "\n";
	}

	public static void addDataPoint(String label, double value) {
		if(data.size() >= 1)
			data.get(label).add(value);
	}

	public static double getLastDataPoint(String label) {
		return data.get(label).size() == 0 ? 0 : data.get(label).get(data.get(label).size() - 1);
	}

	public void setThermistorReading(int reading) {
		thermistor.setText("Thermistor Reading: " + reading);
	}
	
	public void setPhotocellReading(int reading, int number) {
		if (number == FRONT_LEFT)
			photocell1.setText("Front Left Photocell Reading: " + reading);
		else if (number == FRONT_RIGHT)
			photocell2.setText("Front Right Photocell Reading: " + reading);
		else if (number == CENTER)
			photocell3.setText("Center Photocell Reading: " + reading);
	}
	
	private void writeToFile() {
		int option = JOptionPane.showConfirmDialog(null, "Save Data to File?", "Closing Controller", JOptionPane.YES_NO_OPTION);
		
		if (option == JOptionPane.YES_OPTION) {
			try {		
				// Creates data directory if it does not already exist in the project
				File dir = new File(dataDir);
				if (!dir.exists())
					dir.mkdir();

				// Makes file name from current date and time
				Calendar cal = Calendar.getInstance();
				String day = "" + (cal.get(Calendar.MONTH) + 1) + "-" + cal.get(Calendar.DATE) + "-" + cal.get(Calendar.YEAR);
				String time = "" + (cal.get(Calendar.HOUR) == 0 ? 12 : cal.get(Calendar.HOUR)) + "-" + 
				(cal.get(Calendar.MINUTE) < 10 ? "0" + cal.get(Calendar.MINUTE) : cal.get(Calendar.MINUTE)) + "-" +
				(cal.get(Calendar.SECOND) < 10 ? "0" + cal.get(Calendar.SECOND) : cal.get(Calendar.SECOND));
				String filename = dataDir + day + " " + time + ".txt";
				BufferedWriter writer = new BufferedWriter(new FileWriter(new File(filename)));
				// Write Headers
				writer.write(getHeaderLine());
				// Write Data
				for(int i = 0; i < (int) timeCounter; i++) {
					String line = "";
					
					for (String s : labels)
						line += data.get(s).get(i) + "\t";
					
					line = line.substring(0, line.lastIndexOf("\t")) + "\n";
					
					writer.write(line);
				}
				
				writer.flush();
				writer.close();
				
				// Confirmation
				JOptionPane.showMessageDialog(null, "Data written to file: " + filename);
			} catch (IOException e1) {
				JOptionPane.showMessageDialog(null, "Error in writing data to file.");
			}
		}
	}

	public int getMotorSpeed() {
		return motorValue;
	}

	public boolean isRight() {
		return right;
	}

	public boolean isLeft() {
		return left;
	}

	public boolean isForward() {
		return forward;
	}

	public boolean isBackward() {
		return backward;
	}

	@Override
	public void keyPressed(KeyEvent e) {
		int key = e.getKeyCode();
		
		if (key == KeyEvent.VK_RIGHT)
			right = true;
		if (key == KeyEvent.VK_LEFT)
			left = true;
		if (key == KeyEvent.VK_UP)
			forward = true;
		if (key == KeyEvent.VK_DOWN)
			backward = true;

			if (key == KeyEvent.VK_PERIOD) {
				motorValue += motorValue + 24 > 512 ? 0 : 24;
				motorSpeed.setText("Motor Speed: " + motorValue);
			}	
			if (key == KeyEvent.VK_COMMA) {
				motorValue -= motorValue - 24 < -512 ? 0 : 24;
				motorSpeed.setText("Motor Speed: " + motorValue);
			}

		if (key == KeyEvent.VK_ESCAPE) {
			writeToFile();
			System.exit(0);
		}
	}

	@Override
	public void keyReleased(KeyEvent e) {
		int key = e.getKeyCode();
		if(key == KeyEvent.VK_RIGHT)
			right = false;
		if(key == KeyEvent.VK_LEFT)
			left = false;
		if(key == KeyEvent.VK_UP)
			forward = false;
		if(key == KeyEvent.VK_DOWN)
			backward = false;
	}

	@Override
	public void keyTyped(KeyEvent e) {

	}

}
